#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <iostream>
#include <serial_pack/compareAtt.h>
#define rad2deg 57.32484
/*
Copyright(C) 2021 Xiyin,li in hust.
All Rights Reserved.
This file is a node which can listen ur3's tool position and
attitution,it will show as RPY angles in Unit degree.
*/
int main(int argc, char **argv)
{
    ros::init(argc, argv, "publish_ur_tool0");
    tf::TransformListener listener;
    ros::NodeHandle nh;
    ros::Publisher compare_att_pub = nh.advertise<serial_pack::compareAtt>("attFromUR0", 10);
    ros::Rate rate(200);
    while (ros::ok())
    {
        tf::StampedTransform transform;
        try
        {
            listener.waitForTransform("base_link", "tool0", ros::Time(0), ros::Duration(0.001));
            listener.lookupTransform("base_link", "tool0", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex)
        {
            ROS_ERROR_STREAM(ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
        double r;
        double p;
        double y;
        tf::Matrix3x3(transform.getRotation()).getRPY(r, p, y);
        ROS_INFO_STREAM("x" << transform.getOrigin().getX());
        ROS_INFO_STREAM("y" << transform.getOrigin().getY());
        ROS_INFO_STREAM("z" << transform.getOrigin().getZ());
        ROS_INFO_STREAM("==ANGELS==");

        ROS_INFO_STREAM(r * rad2deg);
        ROS_INFO_STREAM(p * rad2deg);
        ROS_INFO_STREAM(y * rad2deg);
        serial_pack::compareAtt result;
        result.pit1 = p*rad2deg;
        result.pit2 = 0;
        result.rol1 = r*rad2deg;
        result.rol2 = 0;
        result.yaw1 = y*rad2deg;
        result.yaw2 = 0;
        //ROS_INFO_STREAM(transform.getRotation().get
        compare_att_pub.publish(result);
        rate.sleep();
    }
}